/*************************************************************
*
* M3D_QUAT.CPP
*
* Quaternions support
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d/m3d.h>

/*****************************************************
* m3dQUAT related funcrions
*****************************************************/

/*****************************************************
* m3dIsEqualQuat_A ()
*
* compare quaternions
*****************************************************/
BOOL m3dIsEqualQuat_A  (const m3dQUAT *a, const m3dQUAT *b, float acc)
{
   if (!m3dIsEqualPoint_A(&a->v, &b->v, acc)) {
      return FALSE;
   }
   if (!m3dIsEqual_A(a->s, b->s, acc)) {
      return FALSE;
   }
   return TRUE;
}

/*****************************************************
* m3dIsZeroQuat_A ()
*
*****************************************************/
BOOL  m3dIsZeroQuat_A (const m3dQUAT *a, float acc)
{
   if (m3dIsEqualPoint_A(&a->v, &m3dVZero, acc)) {
      return TRUE;
   }

   if (m3dIsEqual_A(a->s, 1.f, acc)) { // cos(0.f) = 1
      return TRUE;
   }

   return FALSE;
}

/*****************************************************
* m3dMultiplyQuat ()
*
* Multiply quaternions (see Watt-Watt p.360)
*****************************************************/
void m3dMultiplyQuat (const m3dQUAT * __RESTRICT a, const m3dQUAT * __RESTRICT b, m3dQUAT * __RESTRICT c)
{
//   ASSERT(a != c);
//   ASSERT(b != c);
#if defined(_AP_XENON) && !defined(_NO_XMM)
   XMVECTOR qa = XMLoadFloat4((XMFLOAT4 *)a),
            qb = XMLoadFloat4((XMFLOAT4 *)b);
   qa = XMVectorRotateLeft(qa, 1);
   qb = XMVectorRotateLeft(qb, 1);
   XMVECTOR qc = XMQuaternionMultiply(qb, qa);
   qc = XMVectorRotateRight(qc, 1);
   XMStoreFloat4((XMFLOAT4 *)c, qc);
#else
   c->s = (a->s * b->s) - (a->v.x * b->v.x + a->v.y * b->v.y + a->v.z * b->v.z);
   m3dV cross = { a->v.y * b->v.z - a->v.z * b->v.y,
                  a->v.z * b->v.x - a->v.x * b->v.z,
                  a->v.x * b->v.y - a->v.y * b->v.x };
   c->v.x = a->v.x * b->s + b->v.x * a->s + cross.x;
   c->v.y = a->v.y * b->s + b->v.y * a->s + cross.y;
   c->v.z = a->v.z * b->s + b->v.z * a->s + cross.z;
#endif
}

/*************************************************************
* m3dMultiplyQuat()
*************************************************************/
void    m3dMultiplyQuat      (const m3dQUAT *a, const m3dQUAT *b, float gain, m3dQUAT *c)
{
#if defined(_AP_XENON) && !defined(_NO_XMM)
   XMVECTOR qa = XMLoadFloat4((XMFLOAT4 *)a),
            qb = XMLoadFloat4((XMFLOAT4 *)b);
   qa = XMVectorRotateLeft(qa, 1);
   qb = XMVectorRotateLeft(qb, 1);
   XMVECTOR qc = XMQuaternionMultiply(qb, qa);
   qc = XMVectorRotateRight(qc, 1);
   XMStoreFloat4((XMFLOAT4 *)c, qc);
#else // FPU version
   c->s = (a->s * b->s) - (a->v.x * b->v.x + a->v.y * b->v.y + a->v.z * b->v.z);
   m3dV cross = { a->v.y * b->v.z - a->v.z * b->v.y,
                  a->v.z * b->v.x - a->v.x * b->v.z,
                  a->v.x * b->v.y - a->v.y * b->v.x };
   c->v.x = a->v.x * b->s + b->v.x * a->s + cross.x;
   c->v.y = a->v.y * b->s + b->v.y * a->s + cross.y;
   c->v.z = a->v.z * b->s + b->v.z * a->s + cross.z;
#endif

   m3dV axis;
   float angle;
   c->GetAngAxis(&angle, &axis);
   c->SetAngAxis(angle*gain, &axis);
}

/*************************************************************
* _m3dMultiplyQuat()
*************************************************************/
void _m3dMultiplyQuat (m3dQUAT * __RESTRICT a, const m3dQUAT * __RESTRICT b)
{
   m3dQUAT tmp = *a;
   m3dMultiplyQuat(&tmp, b, a);
}

/*************************************************************
* _m3dClampQuat()
*************************************************************/
void _m3dClampQuat (m3dQUAT * __RESTRICT src, const m3dQUAT * __RESTRICT dest, float maxAngle)
{
   m3dQUAT tmp = *src;
   m3dClampQuat(&tmp, dest, maxAngle, src);
}

/*************************************************************
* m3dDivideQuat()
* c = a*b^(-1)
*************************************************************/
void m3dDivideQuat (const m3dQUAT * __RESTRICT a, const m3dQUAT * __RESTRICT b, m3dQUAT * __RESTRICT c)
{
   m3dQUAT bInv;
   b->Invert(&bInv);

   m3dMultiplyQuat(a, &bInv, c);

   return;
}

/*************************************************************
* m3dClampQuat ()
*************************************************************/
void m3dClampQuat (const m3dQUAT * __RESTRICT src, const m3dQUAT * __RESTRICT dest, float maxAngle, m3dQUAT * __RESTRICT res)
{
   BOOL isSrcZero  = m3dIsZeroQuat(src);
   BOOL isDestZero = m3dIsZeroQuat(dest);
   if (isSrcZero && isDestZero) {
      *res = m3dQUAT();
      return;
   } else if (isSrcZero) {
      *res = *dest;
   } else if (isDestZero){
      src->Invert(res);
   } else {
      m3dDivideQuat(dest, src, res);
   }

   float angle;
   m3dV  axis;
   res->GetAngAxis(&angle, &axis);

   if (!_m3dNormalize(&axis)) {
      *res = m3dQUAT();
      return;
   }

   angle = _m3dClamp(0.f, maxAngle, angle);

   m3dQUAT tmp(angle, &axis);
   if (isSrcZero) {
      *res = tmp;
   } else {
      m3dMultiplyQuat(&tmp, src, res);
   }

   return;
}

/*************************************************************
* m3dLineCombine()
*************************************************************/
void m3dLineCombine (const m3dQUAT * __RESTRICT a, const m3dQUAT * __RESTRICT b, float kb, m3dQUAT * __RESTRICT c)
{
   c->s = a->s + b->s * kb;
   c->v.x = a->v.x + b->v.x * kb;
   c->v.y = a->v.y + b->v.y * kb;
   c->v.z = a->v.z + b->v.z * kb;
}

/*****************************************************
* m3dSlerpQuat ()
*
* Spherical linear interpolation
* between two unit quaternions (see Watt-Watt p.364)
*****************************************************/
void m3dSlerpQuat (const m3dQUAT * __RESTRICT a, const m3dQUAT * __RESTRICT b_, float t, m3dQUAT * __RESTRICT c)
{
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 av = vec_load_unaligned((float*)a);
   vec_float4 bv = vec_load_unaligned((float*)b_);

   // Shuffle s to w
   av = vec_perm(av, av, (vec_uchar16)(vec_uint4){_O_VECTORMATH_PERM_Y, _O_VECTORMATH_PERM_Z, _O_VECTORMATH_PERM_W, _O_VECTORMATH_PERM_X});
   bv = vec_perm(bv, bv, (vec_uchar16)(vec_uint4){_O_VECTORMATH_PERM_Y, _O_VECTORMATH_PERM_Z, _O_VECTORMATH_PERM_W, _O_VECTORMATH_PERM_X});

   vec_float4 start;
   vec_float4 scales, scale0, scale1, cosAngle, angle, tttt, oneMinusT, angles, sines;
   vec_uint4 selectMask;
   cosAngle = vec_dot4( av, bv );
   cosAngle = vec_splat( cosAngle, 0 );
   selectMask = (vec_uint4)vec_cmpgt( (vec_float4)(0.0f), cosAngle );
   cosAngle = vec_sel( cosAngle, negatef4( cosAngle ), selectMask );
   start = vec_sel( av, negatef4( av ), selectMask );
   selectMask = (vec_uint4)vec_cmpgt( (vec_float4)(0.999f), cosAngle );
   angle = acosf4( cosAngle );
   tttt = vec_ld(0, &t);
   tttt = vec_splat(vec_perm(tttt, tttt, vec_lvsl(0, &t)), 0);
   oneMinusT = vec_sub( (vec_float4)(1.0f), tttt );
   angles = vec_mergeh( (vec_float4)(1.0f), tttt );
   angles = vec_mergeh( angles, oneMinusT );
   angles = vec_madd( angles, angle, (vec_float4)(0.0f) );
   sines = sinf4( angles );
   scales = divf4( sines, vec_splat( sines, 0 ) );
   scale0 = vec_sel( oneMinusT, vec_splat( scales, 1 ), selectMask );
   scale1 = vec_sel( tttt, vec_splat( scales, 2 ), selectMask );
   vec_float4 result = vec_madd( start, scale0, vec_madd( bv, scale1, (vec_float4)(0.0f) ) );
   
   // Shuffle s to x
   result = vec_perm(result, result, (vec_uchar16)(vec_uint4){_O_VECTORMATH_PERM_W, _O_VECTORMATH_PERM_X, _O_VECTORMATH_PERM_Y, _O_VECTORMATH_PERM_Z});

   vec_store_unaligned(result, (float*)c);
#elif defined(_AP_XENON) && !defined(_NO_XMM)
   XMVECTOR qa = XMLoadFloat4((XMFLOAT4 *)a),
            qb = XMLoadFloat4((XMFLOAT4 *)b_);
   qa = XMVectorRotateLeft(qa, 1);
   qb = XMVectorRotateLeft(qb, 1);
   XMVECTOR qc = XMQuaternionSlerp(qa, qb, t);
   qc = XMVectorRotateRight(qc, 1);
   XMStoreFloat4((XMFLOAT4 *)c, qc);
#else
   float    omega, cosom, sinom, sclp, sclq;
   float    p[4], q[4], qt[4];
   int      i;
   m3dQUAT  sum, diff, b = *b_;

   m3dAddQuat(a, &b, &sum);
   m3dSubtractQuat(a, &b, &diff);
   if (sum.s  * sum.s  + m3dDotProduct(&sum.v , &sum.v ) <
       diff.s * diff.s + m3dDotProduct(&diff.v, &diff.v)) {
      m3dScaleQuat(&b, -1.f, &b);    
   }

   p[3] = a->s;
   p[0] = a->v.x;
   p[1] = a->v.y;
   p[2] = a->v.z;

   q[3] = b.s;
   q[0] = b.v.x;
   q[1] = b.v.y;
   q[2] = b.v.z;

   cosom = p[0]*q[0]+p[1]*q[1]+p[2]*q[2]+p[3]*q[3];
   if (1.f+cosom>M3D_EPSILON) {
      if (1.f-cosom>M3D_EPSILON) {
         omega = m3dACos(cosom);
         sinom = m3dSin(omega);
         sclp  = m3dSin((1.f - t)*omega)/sinom;
         sclq  = m3dSin(t*omega)/sinom;
      } else {
         sclp  = 1.f-t;
         sclq  = t;
      }
      for ( i = 0; i < 4; i ++) {
         qt[i] = sclp*p[i]+sclq*q[i];
      }
   } else {
      qt[0] = -p[1];
      qt[1] = p[0];
      qt[2] = -p[3];
      qt[3] = p[2];
      sclp = m3dSin((1.f-t)*M3D_PI/2.f);
      sclq = m3dSin(t*M3D_PI/2);
      for ( i = 0; i < 3; i ++) {
         qt[i] = sclp*p[i]+sclq*qt[i];
      }
   }
   c->s   = qt[3];
   c->v.x = qt[0];
   c->v.y = qt[1];
   c->v.z = qt[2];
#endif
}

/*****************************************************
* m3dQUAT Class members
*****************************************************/

/*****************************************************
* m3dQUAT::m3dQUAT()
*
*****************************************************/
m3dQUAT::m3dQUAT (float angle, const m3dV *axis)
{ 
   SetAngAxis(angle, axis); 
}

/*****************************************************
* m3dQUAT::Identity()
*
* Make identity quaternion
*****************************************************/
void m3dQUAT::Identity (void)
{
   this->s   = 1.f;
   this->v.x = 0.f;
   this->v.y = 0.f;
   this->v.z = 0.f;

   return;
}
/*****************************************************
* m3dQUAT::Invert()
*
* Invert a quaternion (see Watt-Watt p.360)
*****************************************************/
void m3dQUAT::Invert (void)
{
   Normalize();
   _m3dScaleVector(&this->v, -1.f);
   return;
}
/*****************************************************
* m3dQUAT::SetAngAxis()
*
* Convert rotation angle and axis to quaternion 
* (see Watt-Watt p.362)
*****************************************************/
void m3dQUAT::SetAngAxis (float angle, m3dV const * __RESTRICT axis)
{
   ASSERT(m3dIsZero_A(m3dLengthVector(axis) - 1.f, 0.001f));
   float sinAngle;

   angle = m3dDeg2Rad(angle/2);

   sinAngle  = m3dSin(angle);
   this->s   = m3dCos(angle);
   this->v.x = sinAngle*axis->x;
   this->v.y = sinAngle*axis->y;
   this->v.z = sinAngle*axis->z;

   ASSERT(m3dFAbs(this->s) <= 1.f + M3D_EPSILON);

   return;
}

/*****************************************************
* m3dQUAT::GetAngAxis()
*
* Convert quaternion to rotation angle and axis 
* (see Watt-Watt p.362)
*****************************************************/
BOOL m3dQUAT::GetAngAxis (float * __RESTRICT angle, m3dV * __RESTRICT axis) const
{
   float sinAngle;
   ASSERT(m3dFAbs(this->s) <= 1.f + M3D_EPSILON);

   float cosIn = this->s;
   {
      if (cosIn > 1.f - M3D_EPSILON) {
         cosIn = 1.f;
      } else if (cosIn < -1.f + M3D_EPSILON) {
         cosIn = -1.f;
      }
   }

   *angle   = m3dACos(cosIn);
   sinAngle = m3dSin(*angle);
   
   *angle  = m3dRad2Deg((*angle)*2.f);

   if (m3dFAbs(sinAngle) < M3D_EPSILON) {
      *axis = m3dVUnitY;
      return FALSE;
   }

   // axis
   axis->x = (float)(this->v.x/sinAngle);
   axis->y = (float)(this->v.y/sinAngle);
   axis->z = (float)(this->v.z/sinAngle);

   return TRUE;
}

/*****************************************************
* m3dQUAT::GetMatr()
*
* RET: pMat - rotation matrix
*
* Convert quaternion to rotation matrix
* (see Watt-Watt p.363)
*****************************************************/
void m3dQUAT::GetMatr (m3dMATR * __RESTRICT pMat) const
{
   GetRotMatr(pMat);

   m3dMatrElementRC(pMat, 0, 3) = 0.f;
   m3dMatrElementRC(pMat, 1, 3) = 0.f;
   m3dMatrElementRC(pMat, 2, 3) = 0.f;
   m3dMatrElementRC(pMat, 3, 3) = 1.f;
   m3dMatrElementRC(pMat, 3, 0) = 0.f;
   m3dMatrElementRC(pMat, 3, 1) = 0.f;
   m3dMatrElementRC(pMat, 3, 2) = 0.f;
}

/***************************************************************
* m3dQUAT::GetRotMatr ()
*
* RET: pMat - matrix with updated rotation component
***************************************************************/
void m3dQUAT::GetRotMatr (m3dMATR * __RESTRICT pMat) const
{
   float q[4];
   q[3] = this->s;
   q[0] = this->v.x;
   q[1] = this->v.y;
   q[2] = this->v.z;

   float s = 2.f / (q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);

   float xs = q[0] * s;
   float ys = q[1] * s;
   float zs = q[2] * s;

   float wx = q[3] * xs; 
   float wy = q[3] * ys;
   float wz = q[3] * zs;

   float xx = q[0] * xs;
   float xy = q[0] * ys;
   float xz = q[0] * zs;
   float yy = q[1] * ys;
   float yz = q[1] * zs;
   float zz = q[2] * zs;

   m3dMatrElementRC(pMat, 0, 0) = 1.f - (yy+zz);
   m3dMatrElementRC(pMat, 0, 1) = xy + wz;
   m3dMatrElementRC(pMat, 0, 2) = xz - wy;

   m3dMatrElementRC(pMat, 1, 0) = xy - wz;
   m3dMatrElementRC(pMat, 1, 1) = 1.f - (xx+zz);
   m3dMatrElementRC(pMat, 1, 2) = yz + wx;

   m3dMatrElementRC(pMat, 2, 0) = xz + wy;
   m3dMatrElementRC(pMat, 2, 1) = yz - wx;
   m3dMatrElementRC(pMat, 2, 2) = 1.f - (xx+yy);
}

/*****************************************************
* m3dQUAT::SetRotMatr()
*
* IN : rotation matrix  (no translation/scale components)
* OUT: quaternion (this)
*
* Convert rotation matrix to quaternion 
* (see Watt-Watt p.363-364)
*****************************************************/
void m3dQUAT::SetRotMatr (const m3dMATR * __RESTRICT rotMatr)
{
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   const vec_float4 zero = (vec_float4){0.0f,0.0f,0.0f,0.0f};
   const vec_float4 one  = (vec_float4){1.0f,1.0f,1.0f,1.0f};
   const vec_float4 half = (vec_float4){0.5f,0.5f,0.5f,0.5f};

   const vec_uint4 SELECT_X = _O_VECTORMATH_MASK_0xF000;
   const vec_uint4 SELECT_Y = _O_VECTORMATH_MASK_0x0F00;
   const vec_uint4 SELECT_Z = _O_VECTORMATH_MASK_0x00F0;
   const vec_uint4 SELECT_W = _O_VECTORMATH_MASK_0x000F;

   const vec_uchar16 PERM_XYCX = _O_VECTORMATH_PERM_XYCX;
   const vec_uchar16 PERM_YCXY = _O_VECTORMATH_PERM_YCXY;
   const vec_uchar16 PERM_CXYC = _O_VECTORMATH_PERM_CXYC;
   const vec_uchar16 PERM_ZXBX = _O_VECTORMATH_PERM_ZXBX;
   const vec_uchar16 PERM_YZAX = _O_VECTORMATH_PERM_YZAX;
   const vec_uchar16 PERM_XZYA = _O_VECTORMATH_PERM_XZYA;
   const vec_uchar16 PERM_ZXXB = _O_VECTORMATH_PERM_ZXXB;
   const vec_uchar16 PERM_YXXC = _O_VECTORMATH_PERM_YXXC;

   const vec_float4 r0 = vec_load_unaligned(&rotMatr->elements[0]);
   const vec_float4 r1 = vec_load_unaligned(&rotMatr->elements[4]);
   const vec_float4 r2 = vec_load_unaligned(&rotMatr->elements[8]);
    
   // 4 cases
   // trace > 0
   // i == 0
   // i == 1
   // i == 2

   const vec_float4 _00_11 = vec_sel(r0, r1, SELECT_Y);
   const vec_float4 _00_11_22_00 = vec_perm(_00_11, r2, PERM_XYCX);
   const vec_float4 _11_22_00_11 = vec_perm(_00_11, r2, PERM_YCXY);
   const vec_float4 _22_00_11_22 = vec_perm(_00_11, r2, PERM_CXYC);
    
   const vec_float4 dsum = vec_add(_00_11_22_00,
                           vec_add(_11_22_00_11,
                                   _22_00_11_22));
   const vec_float4 ddif = vec_sub(_00_11_22_00,
                           vec_add(_11_22_00_11,
                                   _22_00_11_22));

   const vec_float4 traces = vec_add(vec_sel(ddif, dsum, SELECT_W), one);
   
   const vec_float4 isqrt = rsqrtf4(traces); // vec_rsqrte(traces); <-- Too inaccurate.
   const vec_float4 scale = vec_madd(isqrt, half, zero);

   const vec_float4 _12_20_01 = vec_perm(vec_sel(r1, r2, SELECT_X),
                                         r0, PERM_ZXBX);
    
   const vec_float4 _21_02_10 = vec_perm(vec_sel(r0, r2, SELECT_Y),
                                         r1, PERM_YZAX);

    
   const vec_float4 sum = vec_add(_12_20_01, _21_02_10);
   const vec_float4 dif = vec_sub(_12_20_01, _21_02_10);

   // i == 0 j == 1 k == 2
   // res0.x = ...
   // res0.y = 01 + 10
   // res0.z = 02 + 20
   // res0.w = 12 - 21
   vec_float4 res0 = vec_perm(sum, dif, PERM_XZYA);
   // res0.x = trace 
   res0 = vec_sel(res0, traces, SELECT_X);
   // res0.x = trace * (trace ^ -0.5) * 0.5 = sqrt(trace) * 0.5;
   // res0.y = (01 + 10) * s...
   res0 = vec_madd(res0, vec_splat(scale, 0), zero);

   // i == 1 j == 2 k == 0
   // res1.x = 10 + 01
   // res1.y = ...
   // res1.z = 12 + 21
   // res1.w = 20 - 02
   vec_float4 res1 = vec_perm(sum, dif, PERM_ZXXB);
   // res1.y = trace
   res1 = vec_sel(res1, traces, SELECT_Y);
   res1 = vec_madd(res1, vec_splat(scale, 1), zero);

   // i == 2 j == 0 k == 1
   // res2.x = 20 + 02
   // res2.y = 21 + 12
   // res2.z = ...
   // res2.w = 01 - 10
   vec_float4 res2 = vec_perm(sum, dif, PERM_YXXC);
   res2 = vec_sel(res2, traces, SELECT_Z);
   res2 = vec_madd(res2, vec_splat(scale, 2), zero);

   // trace > 0
   // res.x = 12 - 21
   // res.y = 20 - 02
   // res.z = 01 - 10
   // res.w = ...
   vec_float4 res3 = vec_sel(dif, traces, SELECT_W);
   res3 = vec_madd(res3, vec_splat(scale, 3), zero);

   // Now select the correct answer

   const vec_float4 _00 = vec_splat(r0, 0);
   const vec_float4 _11 = vec_splat(r1, 1);
   const vec_float4 _22 = vec_splat(r2, 2);
    
   vec_float4 res = vec_sel(res0, res1, vec_cmpgt(_11, _00));
   res = vec_sel(res, res2,
                 vec_and(vec_cmpgt(_22, _11), vec_cmpgt(_22, _00)));
   res = vec_sel(res, res3, vec_cmpgt(vec_splat(traces, 3), one));

   res = vec_sld(res, res, 12);

   // Uncomment this, remove the #ifdef, and uncomment the bottom test
   // if you suspect something is wrong here.
//   float vmx_result[4];
//   vec_store_unaligned(res, vmx_result);
   vec_store_unaligned(res, reinterpret_cast<float*>(this));
#else
   {
   float   tr,s;
   int     i,j,k;
   int     next[3] = { 1, 2, 0 };
   float   q[4];

   tr = m3dMatrElementRC(rotMatr,0,0) + m3dMatrElementRC(rotMatr,1,1) + m3dMatrElementRC(rotMatr,2,2) +1.f;
   if (tr > 1e-4) {
      s = (float)m3dSqrt(tr);
      q[3] = s*0.5f;
      s    = 0.5f/s;

      q[0] = (m3dMatrElementRC(rotMatr,1,2) - m3dMatrElementRC(rotMatr,2,1))*s;
      q[1] = (m3dMatrElementRC(rotMatr,2,0) - m3dMatrElementRC(rotMatr,0,2))*s;
      q[2] = (m3dMatrElementRC(rotMatr,0,1) - m3dMatrElementRC(rotMatr,1,0))*s;
   } else {
      i = 0;
      if (m3dMatrElementRC(rotMatr,1,1) > m3dMatrElementRC(rotMatr,0,0)) {
         i = 1;
      }
      if (m3dMatrElementRC(rotMatr,2,2) > m3dMatrElementRC(rotMatr,i,i)) {
         i = 2;
      }
      j = next[i];
      k = next[j];

      s = (float)m3dSqrt(m3dMatrElementRC(rotMatr,i,i) - 
                         (m3dMatrElementRC(rotMatr,j,j) + m3dMatrElementRC(rotMatr,k,k)) + 1.f);

      q[i] = s*0.5f;
      s    = 0.5f/s;

      q[3] = (m3dMatrElementRC(rotMatr,j,k) - m3dMatrElementRC(rotMatr,k,j))*s;
      q[j] = (m3dMatrElementRC(rotMatr,i,j) + m3dMatrElementRC(rotMatr,j,i))*s;
      q[k] = (m3dMatrElementRC(rotMatr,i,k) + m3dMatrElementRC(rotMatr,k,i))*s;
   }

   this->s   = q[3];
   this->v.x = q[0];
   this->v.y = q[1];
   this->v.z = q[2];
   }
#endif
/*
    if (fabsf(this->s - vmx_result[0])  > 0.0001f ||
       fabsf(this->v.x - vmx_result[1]) > 0.0001f ||
       fabsf(this->v.y - vmx_result[2]) > 0.0001f ||
       fabsf(this->v.z - vmx_result[3]) > 0.0001f)
    {
       printf("Significant difference between VMX and scalar result!\n");
       printf("this = %g %g %g %g\n", this->s, this->v.x, this->v.y, this->v.z);
       printf("vmx  = %g %g %g %g\n", vmx_result[0], vmx_result[1], vmx_result[2], vmx_result[3]);
       printf("matrix = %g %g %g / %g %g %g / %g %g %g\n",
          rotMatr->elements[0], rotMatr->elements[1], rotMatr->elements[2],
          rotMatr->elements[4], rotMatr->elements[5], rotMatr->elements[6],
          rotMatr->elements[8], rotMatr->elements[9], rotMatr->elements[10]);
       printf("Breakpoint here\n");
    }
*/
   return;

}

/*****************************************************
* m3dQUAT::SetMatr ()
*
* IN : rotation matrix  (no translation/scale components)
* OUT: quaternion (this)
*
* Convert matrix to quaternion 
* (see Watt-Watt p.363-364)
*****************************************************/
void m3dQUAT::SetMatr (const m3dMATR * __RESTRICT pMat)
{
   m3dMATR rotMatr;//, m1;
   float   invsx,invsy,invsz;

   pMat->GetInvScale(&invsx, &invsy, &invsz);

   // This version 25-30% faster than the code below.
   rotMatr.GetAxisX()->x = pMat->GetAxisX()->x * invsx;
   rotMatr.GetAxisX()->y = pMat->GetAxisX()->y * invsx;
   rotMatr.GetAxisX()->z = pMat->GetAxisX()->z * invsx;

   rotMatr.GetAxisY()->x = pMat->GetAxisY()->x * invsy;
   rotMatr.GetAxisY()->y = pMat->GetAxisY()->y * invsy;
   rotMatr.GetAxisY()->z = pMat->GetAxisY()->z * invsy;

   rotMatr.GetAxisZ()->x = pMat->GetAxisZ()->x * invsz;
   rotMatr.GetAxisZ()->y = pMat->GetAxisZ()->y * invsz;
   rotMatr.GetAxisZ()->z = pMat->GetAxisZ()->z * invsz;

   rotMatr.GetOrigin()->x = 0.0f;
   rotMatr.GetOrigin()->y = 0.0f;
   rotMatr.GetOrigin()->z = 0.0f;

   /*
   m1.MakeScale(invsx, invsy, invsz);
   m3dMultiplyMatr(&m1, pMat, &rotMatr);
   rotMatr.Translate(&m3dVZero, M3D_REPLACE);
   */
   SetRotMatr(&rotMatr);

#ifdef _DEBUG
   // These elements aren't required for the maths above,
   // but needed for the assert below.
   m3dMatrElementRC(&rotMatr, 0, 3) = 0.f;
   m3dMatrElementRC(&rotMatr, 1, 3) = 0.f;
   m3dMatrElementRC(&rotMatr, 2, 3) = 0.f;
   m3dMatrElementRC(&rotMatr, 3, 3) = 1.f;
   m3dMatrElementRC(&rotMatr, 3, 0) = 0.f;
   m3dMatrElementRC(&rotMatr, 3, 1) = 0.f;
   m3dMatrElementRC(&rotMatr, 3, 2) = 0.f;

   m3dMATR m1;
   GetMatr(&m1);
   ASSERT(m3dIsEqualMatr_A(&rotMatr, &m1, 0.05f, 0.01f));
#endif
}

/*****************************************************
* m3dQUAT::SetClampRotAxis ()
*****************************************************/
BOOL  m3dQUAT::SetClampRotAxis (const m3dV *norm1, const m3dV *norm2, const m3dV *axis, float maxAngle)
{
   m3dV _axis(*axis);

   if (!_m3dNormalize(&_axis)) {
     // STRONG_ASSERT(FALSE);  @@ TMP
      return FALSE;
   }
   
   float angle = m3dAngleVector(norm1, norm2, axis);

   angle = _m3dClamp(-maxAngle, maxAngle, angle);

   SetAngAxis(angle, axis);

   return TRUE;
}

/*****************************************************
* m3dQUAT::SetClampRotPropAxis ()
*****************************************************/
BOOL  m3dQUAT::SetClampRotPropAxis (const m3dV *norm1, const m3dV *norm2, const m3dV *axis, float speedCoef, float maxStep, float minAngle/* = 0.f*/)
{
   STRONG_ASSERT(maxStep > -M3D_EPSILON);
   STRONG_ASSERT(minAngle > -M3D_EPSILON);

   m3dV _axis(*axis);

   if (!_m3dNormalize(&_axis)) {
      STRONG_ASSERT(FALSE);
      return FALSE;
   }

   float angle = m3dAngleVector(norm1, norm2, axis);
   float stepAngle = angle * speedCoef;
   stepAngle = _m3dClamp(-stepAngle, stepAngle, angle);
   stepAngle = _m3dClamp(-maxStep, maxStep, stepAngle);
   stepAngle = _m3dClamp(0.f, angle - m3dSign(angle)*minAngle, stepAngle);

   if (m3dFAbs(stepAngle) < M3D_EPSILON) {
      Identity();
   } else {
      SetAngAxis(stepAngle, axis);
   }

   return TRUE;
}

/*****************************************************
* m3dQUAT::SetClampRotProp ()
*****************************************************/
BOOL  m3dQUAT::SetClampRotProp (const m3dV *norm1, const m3dV *norm2, float speed, float maxStep, float minAngle/* = 0.f*/)
{
   float    angle;
   m3dV     axis, n1, n2;

   n1 = *norm1;
   n2 = *norm2;
   if (!_m3dNormalize(&n1) || !_m3dNormalize(&n2)) {
      Identity();
      return FALSE;
   }

   m3dCrossProduct(&n1, &n2, &axis);
   if (!_m3dNormalize(&axis)) {
      Identity();
      return FALSE;
   }

   angle = _m3dAngleVector(&n1, &n2);
   float stepAngle = angle * speed;
   stepAngle = _m3dClamp(0.f, maxStep, stepAngle);
   stepAngle = _m3dClamp(0.f, angle, stepAngle);

   stepAngle = _m3dClamp(0.f, angle - minAngle, stepAngle);

   if (m3dFAbs(stepAngle) < M3D_EPSILON) {
      Identity();
   } else {
      SetAngAxis(stepAngle, &axis);
   }

   return TRUE;
}

/*****************************************************
* m3dQUAT::SetClampRot ()
*****************************************************/
BOOL  m3dQUAT::SetClampRot (const m3dV *__RESTRICT norm1, const m3dV *__RESTRICT norm2, float maxAngle)
{
   float    angle;
   m3dV     axis, n1, n2;

   n1 = *norm1;
   n2 = *norm2;
   if (!_m3dNormalize(&n1) || !_m3dNormalize(&n2)) {
      Identity();
      return FALSE;
   }

   m3dCrossProduct(&n1, &n2, &axis);
   if (!_m3dNormalize(&axis)) {
      Identity();
      return FALSE;
   }

   angle = _m3dAngleVector(&n1, &n2);
   angle = _m3dClamp(0.f, maxAngle, angle);

   SetAngAxis(angle, &axis);

   return TRUE;
}

/*****************************************************
* m3dQUAT::SetRot ()
*****************************************************/
BOOL m3dQUAT::SetRot (const m3dV *__RESTRICT norm1, const m3dV *__RESTRICT norm2, float gain)
{
   float    angle;
   m3dV     axis, n1, n2;

   n1 = *norm1;
   n2 = *norm2;
   if (!_m3dNormalize(&n1) || !_m3dNormalize(&n2)) {
      Identity();
      return FALSE;
   }

   m3dCrossProduct(&n1, &n2, &axis);
   if (!_m3dNormalize(&axis)) {
      Identity();
      return FALSE;
   }

   angle = _m3dAngleVector(&n1, &n2);

   SetAngAxis(angle*gain, &axis);

   return TRUE;
}

/*****************************************************
* m3dQUAT::SetRot ()
*****************************************************/
void  m3dQUAT::SetRot (float x, float y, float z, m3dROT_ORDER order)
{
   m3dQUAT rx; rx.SetRotX(x);
   m3dQUAT ry; ry.SetRotY(y);
   m3dQUAT rz; rz.SetRotZ(z);

   switch (order) {
      case M3D_ROT_XYZ:
         m3dMultiplyQuat(&rx, &ry, this);
         _m3dMultiplyQuat(this, &rz);
         break;
      case M3D_ROT_YXZ:
         m3dMultiplyQuat(&ry, &rx, this);
         _m3dMultiplyQuat(this, &rz);
         break;
   }
}

/*****************************************************
* m3dQUAT::SetRotX ()
*****************************************************/
void  m3dQUAT::SetRotX (float angle)
{
   angle = m3dDeg2Rad(angle/2);

   float sinAngle  = m3dSin(angle);
   s   = m3dCos(angle);
   v.x = sinAngle;
   v.y = 0;
   v.z = 0;
}

/*****************************************************
* m3dQUAT::SetRotY ()
*****************************************************/
void  m3dQUAT::SetRotY (float angle)
{
   angle = m3dDeg2Rad(angle/2);

   float sinAngle  = m3dSin(angle);
   s   = m3dCos(angle);
   v.x = 0;
   v.y = sinAngle;
   v.z = 0;
}

/*****************************************************
* m3dQUAT::SetRotZ ()
*****************************************************/
void m3dQUAT::SetRotZ (float angle)
{
   angle = m3dDeg2Rad(angle/2);

   float sinAngle  = m3dSin(angle);
   s   = m3dCos(angle);
   v.x = 0;
   v.y = 0;
   v.z = sinAngle;
}

/*****************************************************
* m3dQUAT::SetRot ()
*****************************************************/
void m3dQUAT::SetRot (const m3dV * __RESTRICT norm1, const m3dV * __RESTRICT norm2)
{
   m3dV n1 = *norm1;
   m3dV n2 = *norm2;

   if (!_m3dNormalize(&n1) || !_m3dNormalize(&n2)) {
      ASSERT(FALSE);
      Identity();
      return;
   }

   m3dV axis;
   m3dCrossProduct(&n1, &n2, &axis);

   if (!_m3dNormalize(&axis)) {
      Identity();
      return;
   }

   float angle = _m3dAngleVector(&n1, &n2);

   SetAngAxis(angle, &axis);
}

/*****************************************************
* m3dQUAT::Normalize ()
*
*****************************************************/
void m3dQUAT::Normalize (void)
{
   float d = GetNorm();

   // null div
   if (d < M3D_EPSILON) {
      return;
   }

   m3dScaleQuat(this, 1.f/d, this);

   return;
}

/*****************************************************
* m3dQUAT::GetNormalized ()
*
*****************************************************/
void m3dQUAT::GetNormalized (m3dQUAT * __RESTRICT quat) const
{
   *quat = *this;
   quat->Normalize();

   return;
}

/*****************************************************
* m3dQUAT::Transform ()
*
*****************************************************/
void m3dQUAT::Transform (const m3dV * __RESTRICT a, m3dV * __RESTRICT res) const
{
   float tmp = s * s - 0.5f;
   res->x = a->x * tmp;
   res->y = a->y * tmp;
   res->z = a->z * tmp;
   tmp = a->x * v.x + a->y * v.y + a->z * v.z;
   res->x += v.x * tmp;
   res->y += v.y * tmp;
   res->z += v.z * tmp;
   m3dV axv = {v.y * a->z - v.z * a->y,
               v.z * a->x - v.x * a->z,
               v.x * a->y - v.y * a->x};
   res->x += axv.x * s;
   res->y += axv.y * s;
   res->z += axv.z * s;
   res->x += res->x;
   res->y += res->y;
   res->z += res->z;
}

/*****************************************************
* m3dQUAT::Transform (const m3dQUAT* quat, m3dQUAT* res)
*
*****************************************************/
void  m3dQUAT::Transform (const m3dQUAT* quat, m3dQUAT* res) const
{
   if (m3dIsZeroQuat_A(this, M3D_EPSILON)) {
      *res = *quat;
      return;
   }
   m3dQUAT qInv, qTemp;
   Invert(&qInv);

   m3dMultiplyQuat(this,  quat,   &qTemp);
   m3dMultiplyQuat(&qTemp, &qInv, res);
}

/*****************************************************
* m3dQUAT::Transform (m3dQUAT* quat)
*
*****************************************************/
void  m3dQUAT::Transform      (m3dQUAT* quat) const
{
   m3dQUAT tmp;
   Transform(quat, &tmp);
   *quat = tmp;
}

/*****************************************************
* m3dQUAT::Transform
*
*****************************************************/
void  m3dQUAT::Transform (m3dMATR* __RESTRICT matr, int operOrder) const
{
   m3dQUAT res = *this;
   switch (operOrder) {
      case M3D_REPLACE:
         break;
      case M3D_PRE:
         {
            m3dQUAT rotCur;
            rotCur.SetMatr(matr);

            m3dMultiplyQuat(this, &rotCur, &res);

            break;
         }
      case M3D_POST:
         {
            m3dQUAT rotCur;
            rotCur.SetMatr(matr);

            m3dMultiplyQuat(&rotCur, this, &res);

            break;
         }
   }


   m3dV translate = *matr->GetOrigin();
   
   res.GetMatr(matr);
   matr->Translate(&translate, M3D_REPLACE);
}

/*************************************************************
* m3dQUAT::GetEulerAngles()
*
* 1st solution http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
* 2nd solution http://www.picamatic.com/view/6383385_rfactor/
*************************************************************/
void m3dQUAT::GetEulerAngles( m3dV *angles ) const
{
   static bool test = false;

   if (test) {
      float half_Pi = M3D_PI / 2;

      m3dMATR mRot;
      this->GetRotMatr(&mRot);

      angles->x = m3dASin( - mRot._32);

      if(angles->x < half_Pi)
      {
         if(angles->x > - half_Pi)
         {
            angles->y = m3dATan2(mRot._31, mRot._33);
            angles->z = m3dATan2(mRot._12, mRot._22);
         }
         else
         {
            angles->y = -m3dATan2( - mRot._21, mRot._11);
            angles->z = 0.0f;
         }
      }

      m3dScaleVector(angles, 180.f/M3D_PI, angles);

   } else {
      m3dQUAT q;
      GetNormalized(&q);

      float test = q.v.x * q.v.y + q.v.z * q.s;

      if (test > 0.5 - M3D_EPSILON) {
         // singularity at north pole
         angles->y = 2.f * m3dRad2Deg(m3dATan2(q.v.x, q.s));
         angles->x = 90.f;
         angles->z = 0.f;
         return;
      }
      if (test < -0.5 + M3D_EPSILON) { 
         // singularity at south pole
         angles->y = -2.f * m3dRad2Deg(m3dATan2(q.v.x, q.s));
         angles->x = -90.f;
         angles->z = 0.f;
         return;
      }

      float sqx = q.v.x * q.v.x;
      float sqy = q.v.y * q.v.y;
      float sqz = q.v.z * q.v.z;

      float denom = 1.f - 2.f * (sqy + sqz);
      angles->y = m3dRad2Deg(m3dATan2(2.f * (q.v.y * q.s - q.v.x * q.v.z) , denom));
      angles->z = m3dRad2Deg(m3dASin(2.f * test));

      denom = 1.f - 2.f * (sqx + sqz);
      angles->x = m3dRad2Deg(m3dATan2(2.f * (q.v.x * q.s - q.v.y * q.v.z) , denom));
   }
}

/**************************************************************
* m3dQUAT::Check()
*
**************************************************************/
BOOL m3dQUAT::Check (void) const
{
   if (m3dFAbs(this->s) >= 1.f + M3D_EPSILON) {
      return FALSE;
   }

   return TRUE;
} 

/**************************************************************
* m3dQUAT::Clamp()
*
**************************************************************/
void m3dQUAT::Clamp (float gain)
{
   float ang;
   m3dV axis;
   GetAngAxis(&ang, &axis);
   SetAngAxis(ang*gain, &axis);
}

//
// End-of-file M3D_QUAT.CPP
//
